Spatial avoidance method and apparatus

ABSTRACT

An obstacle avoidance system, apparatus, and method is described suitable for use on autonomously guided or man-in-the-loop guided vehicles such as aircraft, missiles, cars, and other types of vehicles. The system provides guidance instructions in the situation where the vehicle encounters an obstacle either directly or indirectly in its path while traveling to a desired destination or where vehicles are traveling in formation. The system can be applied not only to obstacle avoidance but also to trajectory shaping by defining obstacles, operational boundaries and/or threats which influence the trajectory of the vehicle.

RELATED APPLICATIONS

[0001] This application claims the benefit of U.S. Provisional PatentApplication Ser. No. 60/171,314, filed on Dec. 21, 1999, the entirecontents of which are hereby incorporated by reference.

FIELD OF THE INVENTION

[0002] The present invention relates to a guidance method and systemsuitable for use with autonomously guided and man-in-the-loop guidedvehicles where the presence of obstacles and/or threats must beconsidered in guiding the vehicle to a destination.

BACKGROUND OF THE INVENTION

[0003] During the travel of an autonomously guided or man-in-the-loopguided vehicle towards a destination, there may be obstacles,impediments and/or threats present in the path of travel which must beavoided which otherwise would prevent the vehicle from reaching itsdestination or would cause damage to the vehicle. Various guidanceaugmentation concepts have been suggested for avoiding obstacles,operational boundaries, and/or threats. Generally, these concepts havebeen developed with the approach such that if no impediments arepresent, then the travel of the guided vehicle is not changed. However,if impediments are present, these concepts provide the guidance systemwith directions diverting the vehicle around the impediment to allow thevehicle to proceed to the desired destination.

[0004] These avoidance methods have been used, for instance, in thefield of tactical missiles. There, during the launch, flight, andintercept phases of flight, there are numerous constraints on theguidance system that result from obstacles or operational limitationsthat must be accommodated in addition to the requirement that themissile be guided so as to hit the target. Some of these constraintsinclude maneuverability constraints based on dynamic, kinematic, and/ormechanical constraints of the vehicle; operational envelope constraintssuch as maximum altitude or minimum altitude limitations on the vehicle;obstacle constraints, such as friendly airborne assets; and, threatconstraints such as enemy assets which may target the vehicle in anattempt to destroy the vehicle. All of these factors are significantconsiderations in shaping the trajectory of the missile during itsflight. Various textbook guidance methods that have been proposed willbe briefly discussed below. These methods, however, do not take intoaccount all of those constraints and therefore would benefit fromaugmentation in some form prior to implementation in a real worldapplication.

[0005] A first known method for accommodating guidance constraints isknown as a waypoint method. This guidance method uses waypoints orintermediate points along a trajectory. One such concept is described inthe article “Obstacle-Avoidance Automatic Guidance: A ConceptDevelopment Study” by Victor H. L. Cheng, published in AIAA Journal,Paper Number 88-4189-CP, 1988. The basic idea behind the waypointconcept is that if an obstacle, operational boundary, or threat isperceived, a set of waypoints is constructed such that an avoidance pathcan be achieved. This concept is illustrated in FIG. 1. As shown in FIG.1, a vehicle 10 can be navigated from a starting point 110 to adestination 140 via waypoints 120 and 130. To generate an efficienttrajectory, the waypoints must be selected carefully. The waypointmethod is suitable for applications where the impediments are stationaryand are not dynamic.

[0006] Another known avoidance method is to use optimal guidancestrategies to steer a vehicle around obstacles or boundaries. One suchapproach is described in the article “Control Theoretic Approach to AirTraffic Conflict Resolution”, AIAA Journal, Paper Number 93-3832-CP,1993. This method involves the definition of a cost function thatmeasures the quality or goodness of a particular trajectory and whichoptimizes the cost function over a set of possible trajectories. Theconstruction of the cost function is generally based on the dynamics orrelative motion of the obstacles, operational boundaries, and/or threatsto the missile. Generally, this method must be solved numerically andcan require large amounts of real time processing to solve. Also, theaddition of multiple obstacles, operational boundaries, and/or dynamicthreats in the operation or flight significantly increases thecomplexity of the cost function and thus further increases processingrequirements.

[0007] Another avoidance method relies upon mathematical representationsof potential fields. In this approach, sources, which are potentialfield elements that provide a mathematically repelling force, can beused individually or as a surface to provide a range dependent force inorder to push the missile guidance away from the obstacle. Sources havea unique quality in that the amount of repelling force is inverselyproportional to the distance from the source. For example, a missilethat is close to a source will be pushed away with greater force than amissile that is far from the source. Because of this range dependentcharacteristic, and because sources are computationally efficient touse, sources can be used in a large number of avoidance applications.However, source methods have a drawback that, if they are not modified,they will affect the guidance commands by directing the vehicle awayfrom the obstacle throughout the trajectory of the vehicle because thesource has an infinite range of influence.

[0008] A modification to the source approach adds a range boundarybeyond which the source will not affect the vehicle. One such approachis described in “Generation of Conflict Resolution Maneuvers for AirTraffic Management”, by Claire, et al., IEEE Journal, Paper No.0/7803-4119-8, 1997. In this approach, the method will push the guidancecommand away from the obstacle only when the missile is inside the rangeboundary and will not affect the vehicle when it is outside theboundary. One drawback to this method is that the guidance command ismathematically discontinuous across the boundary. Discontinuous commandsare undesirable because they can cause instabilities in the guidancesolution in the circumstance when multiple obstacles, boundaries, orthreats are encountered. Another drawback to this method is in regard tothreat avoidance. Specifically, this method will tend to direct avehicle along the direction of the velocity of the threat in order toavoid the threat. However, if the threat has sufficient velocity toovertake the vehicle and the source potential is located on the vehicle,the threat can nevertheless have a successful intercept. The Claire, etal. article also mentions the use of vortex potential field elements inthe construction of an avoidance solution. Vortex elements areessentially planar rotation elements that push the field in a rotationaldirection about a point. Vortex elements are not easily adapted tomulti-dimensional spaces above two dimensions, because they aretwo-dimensional elements. To apply a vortex element in athree-dimensional space would require knowledge of the direction whichthe vortex would act about. In a dynamic situation, the addition of avortex element would limit the number of evasive solutions and wouldhinder the performance of an avoidance algorithm. Nevertheless,two-dimensional applications of vortex elements can be appropriate incircumstances such as traffic management applications where the flow ofvehicles conforms to pre-established guidelines.

[0009] Another known avoidance method employs specific geometricboundary shapes to describe how an avoidance maneuver is to beperformed. These geometric methods use the surface tangents from thegeometric boundary to provide a direction of avoidance for the avoidancemaneuver. One such avoidance approach is described in “ASelf-Organizational Approach for Resolving Air Traffic Conflicts”, pages239-254, by Martin S. Eby, published in The Lincoln Laboratory Journal,Volume 7, No. 2, 1994. Such an approach is illustrated in FIG. 2, wherea vehicle 10 is traveling in direction of arrow D and must avoidobstacle plane 200. A geometric boundary shape 210 is defined aroundobstacle plane 200 at a radius r_(D) which is the desired miss distance.In the case where the path of the subject plane 10 to its destination220 intersects the geometric boundary shape 210, an avoidance maneuveris necessary. In this circumstance, an avoidance velocity vectorsolution V_(S) is found, in part, by forming a tangent 230 to thegeometric boundary 210. A significant limitation to this method withregard to tactical missiles is that if the missile gets inside of theavoidance geometric boundary, then the tangent vector to the surface nolonger exists. The Claire, et al. article also discusses how the size ofthe geometric boundary shape can be reduced in order to allow forcalculation of a tangent. Specifically, in the case where the missilegets inside of the avoidance geometric boundary, the geometric boundaryshape is reduced until the missile is returned to the exterior of thegeometric boundary. The reduction of the size of the geometric boundarycan work for obstacle avoidance because as a subject vehicle approachesthe surface geometry, the guidance direction becomes more aggressive inan avoidance direction.

[0010] One shortcoming of the method occurs when trying to provide anoperational boundary such as a maximum altitude for a vehicle. Onerepresentation of an absolute altitude ceiling is a single sphere whichsurrounds the earth at a specified altitude. Such a sphere, however,would immediately cause a problem with the above-described methodbecause the vehicle would always be inside the geometric boundary andthus no tangent could be formed. Such a method would also havedifficulty with an altitude ceiling limitation if a maximum altitudewere represented by a plane. In this case, the geometric tangent methodwould provide an avoidance command that was always parallel to theplane. As a result, the desired avoidance command would be ignored atall times.

[0011] The tangent geometric boundary avoidance technique is alsoinadequate in the circumstance of launching a missile from an aircraft.Specifically, the tangent method cannot accommodate the situation wherethe missile is inside the geometric shape which surrounds the vehicle tobe avoided. Thus, the geometric shape must be made very small. However,if the geometric shape surrounding a launching aircraft is made smallenough to allow the tangent method to work, the geometric shape would besmaller than the aircraft to be avoided, no avoidance commands would beissued, and the missile would be able to fly into the launchingaircraft.

[0012] Accordingly, in view of the above, what is needed is an improvedavoidance system and method which can address the shortcomings outlinedabove.

SUMMARY OF THE INVENTION

[0013] An object of the present invention is to provide an avoidancesystem and method which addresses both static and dynamic obstacles andwhich does not require specialized algorithms for dynamic obstacles.

[0014] Another object of the present invention is to provide an obstacleavoidance system and method which provide a deterministic avoidancecommand and which does not require an iterative minimization algorithmfor a solution.

[0015] A further object of the present invention is an avoidance systemand method which comprehends the situation where the vehicle is insideof a geometric boundary shape.

[0016] According to the present invention, the foregoing objects areattained by blending together a distance-based field element (such assource, sink, or vortex), a geometric boundary surface normal andtangent, an original guidance command, and the distance to the obstacleto produce an avoidance command that will direct the missile away fromthe obstacle and proceed in the general direction of the targetdestination.

[0017] The avoidance commands are constructed in a fashion such that, inthe case where a vehicle is very close to an obstacle, the avoidancecommand directs the vehicle in a direction to prevent collision. When avehicle approaches a geometric boundary, the avoidance command isblended from an anti-intercept direction to a direction that is afunction of a surface tangent, such that the avoidance command iscontinuous across the geometric boundary. Outside the geometricboundary, the guidance command is blended with a vector that is afunction of the surface tangent taken in a distance dependent fashion toprovide an avoidance direction along the desired direction of travel. Asa result, the inclusion of obstacles and/or threats in the operationalspace of a vehicle will produce an alteration to the nominal guidancecommands yet still allow the vehicle to travel to its intended finaldestination or final state. In the absence of obstacles, operationalboundaries or threats, the present invention will not alter the originalguidance commands and the vehicle will travel according to the originalguidance implementation.

[0018] This invention may be applied in applications such as autonomousvehicles as well as vehicles employing man-in-the-loop guidance.Specifically, such as in the situation of a flight director, an operatorof the vehicle may be informed of a proper guidance adjustment requiredto avoid an obstacle.

BRIEF DESCRIPTION OF THE DRAWINGS

[0019] A more complete understanding of the present invention and itsadvantages will be readily apparent from the following DetailedDescription taken in conjunction with the accompanying drawings.Throughout the accompanying drawings, like parts are designated by likereference numbers.

[0020]FIG. 1 shows an example of a vehicle being maneuvered along aflight path by the method of waypoints as known in the prior art;

[0021]FIG. 2 shows a geometric boundary avoidance method of the priorart;

[0022]FIG. 3A shows a first embodiment where an avoidance command isdetermined in a case where the object is contained inside a geometricboundary and where the vehicle is outside the geometric boundary;

[0023]FIG. 3B shows a detailed view of the vector analysis used in afirst embodiment where an avoidance command is determined in a casewhere the object is contained inside a geometric boundary and where thevehicle is outside of the geometric boundary;

[0024]FIG. 3C shows a detailed view of the vector analysis used in afirst embodiment where an avoidance command is determined in a casewhere the object is contained inside a geometric boundary and where thevehicle is outside of the geometric boundary and has passed the object;

[0025]FIG. 4 shows a first embodiment where an avoidance command isdetermined in the case where the object is contained inside a geometricboundary and where the vehicle is also inside the geometric boundary;

[0026]FIG. 5 shows an operational boundary, such as a minimum altitude,to prevent the vehicle from flying below a minimum altitude boundarylimit;

[0027]FIG. 6 shows an operational boundary, such as a maximum altitude,to prevent the vehicle from climbing above the boundary limit;

[0028]FIG. 7 shows an operational boundary to prevent a vehicle, such asa submarine, from entering a specified boundary area;

[0029]FIG. 8 shows a vehicle avoidance process using the presentinvention in the situation where the vehicle is being pursued by athreat;

[0030]FIG. 9A illustrates a missile launch from an aircraft withoutusing the present invention to prevent the missile from impacting thelaunching aircraft;

[0031]FIG. 9B shows an application of a missile launch from an aircraftusing the present invention to prevent the missile from impacting thelaunching aircraft;

[0032]FIGS. 10A and 10B show an application of the present invention tocontrol vehicles traveling together in formation;

[0033]FIGS. 10C through 10E show another application of the presentinvention to control vehicles traveling together in formation;

[0034]FIG. 11 is a vector field diagram representing a situation whereonly one obstacle is present;

[0035]FIG. 12 is a vector field diagram representing a situation wheremultiple obstacles are present in the vector field;

[0036]FIG. 13 is a vector field diagram representing the presence ofmultiple obstacles in the vector field with intersecting obstacleboundaries;

[0037]FIG. 14 is a vector field diagram representing the presence ofmultiple obstacles having finite size with intersecting obstacleboundaries;

[0038]FIG. 15 is a vector field diagram representing the presence ofmultiple obstacles having finite size with intersecting obstacleboundaries where the destination is inside a geometric boundary;

[0039]FIG. 16 is a vector field diagram representing the presence ofmultiple obstacles having finite size with intersecting obstacleboundaries and the presence of a non-closed infinite boundary surface;

[0040]FIG. 17 is a vector field diagram representing the presence ofmultiple obstacles having finite size with intersecting obstacleboundaries, the presence of a non-closed infinite boundary surface, andwith tangent boundary direction control;

[0041]FIG. 18 is a vector field diagram representing the presence ofmultiple obstacles having finite size with intersecting obstacleboundaries, the presence of a non-closed infinite boundary surface withtangent boundary direction control, and with multiple potential types;

[0042]FIG. 19 is a vector field diagram illustrating application of thepresent invention for use as an automated flow manager in trafficcorridor;

[0043]FIG. 20 is a vector field diagram illustrating an application ofthe present invention in a Six-Degree-of-Freedom (6DOF) vehicle controlapplication;

[0044]FIG. 21 shows construction of the components for the 6DOF guidancemethod;

[0045]FIG. 22 is a block diagram of an autonomous vehicle guidanceprocessing unit;

[0046]FIG. 23 is a block diagram of an automatic guidance processingunit for a piloted vehicle; and

[0047]FIG. 24 is a block diagram of a pilot controlled guidanceprocessing unit for a piloted vehicle.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0048] Preferred Embodiments of the present invention will now bedescribed in detail in reference to the various drawings.

[0049] As noted above, an avoidance method and system in accordance withthe present invention combine the ability to avoid obstacles whileallowing the vehicle to proceed to a final desired destination. In thecase of a tactical missile, the primary mission of the missile is to flyto a target destination, which may be in motion or at rest, and todeliver at the destination a payload. Along the trajectory, the missilemay encounter obstacles or impediments, which may be in the form offriendly airborne assets, operational boundaries which may definealtitude ceilings and floors, and threats, which may try to interceptthe missile during its mission. The objective of the avoidance methodand system is to provide a guidance command or direction (e_(avoid))which can direct the missile so as to avoid the obstacles and stillpermit the vehicle to reach the intended target destination.

[0050] In order to operate, the present method and system requireinformation about the state of each obstacle. Such information mayinclude the obstacle's position, velocity and heading, acceleration, orsome other quantity which can be used in the avoidance methodcomputation. The method is not dependent on how the information isobtained, however, the effectiveness of the resulting obstacle avoidancecommands which result from the present method will be influenced by theaccuracy of the state information provided with regard to theobstacle(s).

FIRST EMBODIMENT Obstacle Avoidance

[0051] A First Embodiment of the present invention is described inregard to an implementation where an obstacle is contained inside ageometric boundary.

[0052] Vehicle Outside a Geometric Boundary of the Object

[0053] A First Embodiment of the present invention involvesimplementation of the obstacle avoidance method computation where thevehicle to be maneuvered to avoid the obstacle is outside a geometricboundary of the obstacle as shown in FIG. 3A. The geometric boundaryincludes the position of the obstacle and a region surrounding theobstacle. FIG. 3A generally illustrates the situation where a vehicle 10is approaching an object to be avoided 300, where the object to beavoided 300 is contained inside a geometric boundary 310 and where thevehicle 10 is outside the geometric boundary 310. Various guidancevectors “e” are shown in FIG. 3A, and will be individually described inmore specific detail below.

[0054] It should be noted that in the following discussion, when theboundary defines an object or region to be avoided, the object and itssurrounding region are referred to as a source element. When theboundary defines an object or region towards which the vehicle isattracted, the object or region is referred to as a sink element. Forthe purpose of understanding the present method, however, the repellingeffect of a source or the attracting effect of a sink are differentprincipally in the sign (i.e., + or −) of the forces and vectorsinvolved. The fundamental principles described next are otherwise thesame for sources and sinks.

[0055] As illustrated in FIG. 3A, vehicle 10 travels at velocity V_(p)following a nominal guidance command e_(desired) which represents theguidance command that vehicle 10 would be commanded to follow in theabsence of any obstacle whose presence would cause a deviation. Obstacle300 may be moving at a velocity V_(imp). However, for purposes ofdiscussion at this time, V_(imp) is substantially zero. Vector e_(int)represents a line of intercept between the obstacle 300 and the vehicle10. Point R represents the point of intersection of vector e_(int) onthe surface of the geometric boundary 310. Plane 320 is a plane which istangent to the geometric boundary 310 at point R, and point R is thesurface point of tangency of plane 320. Vector e_(N) is a surface normalvector at the surface point of tangency, R. Vector e_(tan) is a vectorrepresenting a component of the nominal guidance command e_(desired)projected onto tangent plane 320 and is the component in the directionof the nominal guidance command e_(desired). As will be explained morefully below, plane 330 represents a transition zone between a statewhere the vehicle 10 is approaching the obstacle 300 and a state wherethe vehicle 10 has overtaken and is receding from the obstacle 300.Vector e′_(tan) is the geometric based avoidance vector which isdetermined from e_(N) and e_(tan). Vector e_(avoid) represents theresulting avoidance guidance command, the computation of which will beexplained below in reference to FIG. 3B.

[0056]FIG. 3B more specifically illustrates the applicable vectors usedin determining the resulting avoidance guidance command e_(avoid) in asituation similar to that shown in FIG. 3A, where an object to beavoided is contained inside a geometric boundary and the vehicle isoutside the geometric boundary. For discussion purposes, all vectors inFIG. 3B are shown originating at R to demonstrate the relativerelationship between the vectors.

[0057] As illustrated in FIG. 3B, a plane 320 tangent to a geometricsurface of the geometric boundary 310 is defined having its point oftangency at point R, which is the intersection of the geometric surfaceof the geometric boundary with the vector e_(int) defined by therelative position of a vehicle (not shown but positioned as if at R) tothe obstacle 300. Vector e_(tan) is a component of the nominal guidancecommand, e_(desired), when e_(desired) is projected onto plane 320 sothat e_(tan) is in the direction of the nominal guidance command.

[0058] As shown in equations (1a) and (1b), the surface normal e_(N) andthe tangent direction at the surface point of tangency e_(tan) are thencombined through the use of linear or non-linear functions h₁(x) andh₂(x) to provide a geometric-based avoidance vector e′_(tan). Thisapproach blends vector e_(N) with vector e_(tan) such that the geometricavoidance vector e′_(tan) can range from e_(tan) to e_(N) depending onthe value of functions h₁ (x) and h₂ (x). According to the FirstEmbodiment,

e′ _(tan) =E′ _(tan) /∥E′ _(tan)∥  (1a)

[0059] where

E′ _(tan) =h ₁(x)e _(N) +h ₂(x)e _(tan)  (1b)

[0060] where 0≦h₁(x)≦∞ and 0≦h₂(x)≦∞, such that h₁(x) and h₂(x) are notboth zero. For all equations of the form above, functions such as h₁(x)and h₂(x) should be constructed such that ∥E′_(tan)∥≠0.

[0061] One particular example of functions h₁(x) and h₂(x) whichsatisfies the above criteria is where h₂(x) is defined as 1−h₁(x), where0≦h₁(x)≦1. However, in the obstacle avoidance method of the presentinvention, functions h₁(x) and h₂(x) need not be related. Any otheravoidance functions may alternately be used in the computation so longas they satisfy the above requirements.

[0062] Referring to FIGS. 3A and 3B, after geometric avoidance vectore′_(tan) is determined, the resulting avoidance guidance commande_(avoid) is determined, as shown in equations (2a) and (2b), byblending together e′_(tan) with e_(desired), based on functions f₁(x)and f₂(x), such that e_(avoid) can range from e_(desired) to e′_(tan)depending on the value of functions f₁(x) and f₂(x). According to theFirst Embodiment,

e _(avoid) =E _(avoid) /∥E _(avoid)∥  (2a)

[0063] where

E _(avoid) =f ₁(x)e _(desired) +f ₂(x)e′ _(tan)  (2b)

[0064] where 0≦f₁(x)≦∞ and 0≦f₂(x)≦∞, such that f₁(x) and f₂(x) are notboth zero; and where f₂(x)=0, when the vehicle is at or beyond point L,point L being a point on the influence surface 309 located a distancefrom the obstacle 300 to be avoided along e_(int); and f₁(x)=0, when thevehicle 10 is at point R. Further, functions f₁(x) and f₂(x) should beconstructed such that ∥E_(avoid)∥≠0.

[0065] One particular example of functions f₁(x) and f₂(x) is wheref₂(x) is defined as 1−f₁(x), where 0≦f₁(x)≦1. However, in the obstacleavoidance method of the present invention, functions f₁(x) and f₂(x)need not be related. Other avoidance functions may alternately be usedin the computation.

[0066] As noted above for FIG. 3A, plane 330 represents a transitionzone for changing the avoidance command as the vehicle 10 overtakes andpasses the obstacle 300. More specifically, when the vehicle is outsidethe geometric boundary 310 of the obstacle 300, if the avoidanceguidance command e_(avoid), as described and determined above, isapplied as the vehicle 10 overtakes and passes the obstacle 300, theguidance command generated will unnecessarily and undesirably follow thee_(tan) vector after passing the obstacle 300.

[0067] Referring now to FIG. 3C, once the vehicle 10 has passed theobstacle 300, the e′_(tan) vector can be blended with the e_(desired)vector by using continuous linear or non-linear functions g₁(x) andg₂(x) to construct e_(tan)*. The vector e_(tan)* is constructed based one′_(tan) and e_(desired) through functions g₁(x) and g₂(x) so thate_(tan)*→e′_(tan) when the vehicle 10 is approaching the obstacle 300and e_(tan)*→e_(desired) after the vehicle 10 passes the obstacle 300while remaining outside the geometric boundary 310. Like functions,h₁(x) and h₂(x), functions g₁(x) and g₂(x) may be limited to operationover a specified range, γ.

[0068] Vector e_(tan)* can be described in accordance with functions(3a), (3b) and as follows:

e _(tan) *=E _(tan) */∥E _(tan)*∥  (3a)

[0069] where

E _(tan) *=g ₁(x)e _(desired) +g ₂(x)e′ _(tan)  (3b)

[0070] where 0≦g₁(x)≦∞ and 0≦g₂(x)≦∞, such that g₁(x) and g₂(x) are notboth zero; and g₂(x)=0, when the vehicle 10 is at an end 306 of thetransition range γ, and g₁(x)=0 when the vehicle 10 is at a beginning307 of the transition range γ. Further, functions g₁(x) and g₂(x) shouldbe constructed such that ∥E_(tan)∥≠0.

[0071] In an exemplary embodiment, g₁(x)=g₁(x, γ) and g₂(x)=g₂(x, γ)where g₂(x, γ)=1−g₁(x, γ) and where γ is a continuous multi-variablefunction which describes the spatial boundary over which the commandwill be transitioned from e_(desired) to e′_(tan).

[0072] Thus, in equations above, when functions g₁(x) and g₂(x) are usedto modify the behavior of the avoidance command as the vehicle 10 passesthe obstacle 300, e_(tan)* may then be substituted into equation (2b) toyield equation (4b), shown below, which is a more robust solution forobtaining e_(desired) and which addresses the desirable transition ofthe guidance command after the vehicle 10 passes the obstacle 300.

E _(avoid) =f ₁(x)e _(desired) +f ₂(x)e _(tan)*  (4b)

[0073] where f₁(x) and f₂(x) are as defined above in regard to equations(2a) and (2b), and where e_(tan)* is determined as defined above inregard to equations (3a) and (3b).

[0074] As was discussed above in previous computations of the method,e_(avoid) is computed by the following equation:

e _(avoid) =E _(avoid) /∥E _(avoid)∥  (4a).

[0075] It will be appreciated by those skilled in the art that allfunctions described herein may be either linear or non-linear continuousfunctions. Also, for simplicity, the parameter, x, is used as thevariable parameter of all functions throughout the discussion of theobstacle avoidance method computation. The variable “x” may represent aparameterized constant or set of constants, varying function, or familyof functions. For example only and not by way of limitation, “x” mayrepresent position, distance, time, angle, and/or acceleration. It willbe appreciated by those skilled in the art that “x” may representmultiple functions or a multi-variable function, e.g. d(x) may insteadbe d(a, b) or d(a, b, y). It will further be appreciated by thoseskilled in the art that “x” need not represent the same parameter or setof parameters in every function of an equation, nor need it representidentical variables in functions of different equations of the obstacleavoidance method computation.

[0076] Both Vehicle and Object are Inside a Geometric Boundary

[0077] The situation discussed above relative to FIGS. 3A, 3B and 3Caddresses the situation where the object to be avoided 300 is containedinside of a geometric boundary 310 and where the approaching orretreating vehicle 10 is outside the geometric boundary 310. As will bediscussed next with regard to FIG. 4, the First Embodiment of thepresent invention can also easily address the situation where both thevehicle 10 and the object to be avoided 300 are inside of a geometricboundary 310

[0078]FIG. 4 generally illustrates the situation where a vehicle 10 isapproaching an object to be avoided 300, where both the object to beavoided 300 and the vehicle 10 are contained inside the geometricspatial boundary 310. Various guidance vectors “e” shown in FIG. 4generally correspond to the guidance vectors shown in FIGS. 3A and 3Band will be more specifically described below.

[0079] Vehicle 10 travels at velocity V_(p) following a nominal guidancecommand e_(desired) which represents the guidance command that vehicle10 would be commanded to follow in the absence of any obstacle whosepresence would cause a deviation. Vector V_(imp) represents the velocityof the obstacle 300 and is considered for illustrative purposes only tobe zero. Vector e_(int) represents a line of intercept between thevehicle 10 and the obstacle 300. Point R represents the point ofintersection of vector e_(int) on the surface of the geometric boundary310. Plane 320 is a plane which is tangent to the geometric boundary 310at the surface point of tangency R. Vector e_(N) is a surface normalvector at the surface point of tangency R. Vector e_(tan) is a vectorrepresenting the projection of the nominal guidance command e_(desired)onto tangent plane 320 and is in the direction of the nominal guidancecommand e_(desired). As shown in FIGS. 3A and 4, plane 330 represents atransition zone between a state where the vehicle 10 is approaching theobstacle 300 and a state where the vehicle 10 has overtaken and isreceding from the obstacle 300. Vector e′_(tan) is the geometric basedavoidance vector which is determined from e_(N) and e_(tan). Vectore_(avoid) represents the resulting avoidance guidance command, thecomputation of which will be explained below.

[0080] In the case where the vehicle 10 is inside the geometric boundary310 (FIG. 4), as with the case where the vehicle 10 is outside thegeometric boundary surface 310 (FIG. 3A), a geometric-based avoidancevector e′_(tan) is determined in accordance with equations (1a) and (1b)by blending the surface normal e_(N) and the tangent direction at thesurface point of tangency e_(tan) through the use of the functions h₁(x)and h₂(x). As noted above, this approach blends vector e_(N) with vectore_(tan) such that the geometric avoidance vector e′_(tan) can range frome_(tan) to e_(N) depending on the value of functions h₁(x) and h₂(x). Toavoid discontinuity in the calculation at the surface of the geometricboundary for the case where the vehicle 10 is inside the geometricboundary 310, it is preferable that h₁(x) and h₂(x) be the samefunctions as h₁(x) and h₂(x) in the case where the vehicle 10 is outsidethe geometric boundary 310. If the discontinuity is large enough, it mayproduce an unstable guidance mode whereby the guidance command willoscillate about a surface of the geometric boundary 310 and not producethe desired avoidance effect.

[0081] After the geometric avoidance vector e′_(tan) is determined, theresulting avoidance guidance command e_(avoid) is determined. Unlike theapproach shown in equations (2a) and (2b), which are applied in thesituation where the vehicle 10 is outside the geometric boundary 310, inthe case where the vehicle 10 is inside of the geometric boundary 310,e_(avoid) is determined based on e′_(tan) and e_(int). Morespecifically, as shown in equations (5a) and (5b), e_(avoid) isdetermined by blending together e′_(tan) with e_(int), based onfunctions f′₁(x) and f′₂(x), such that e_(avoid) can range from e_(int)to e′_(tan) depending on the value of the functions f′₁(x) and f′₂(x).In this situation,

e _(avoid) E _(avoid) /∥E _(avoid)∥  (5a)

[0082] where

E _(avoid) =f′ ₁(x)e _(int) +f′ ₂(x)e′ _(tan)  (5b)

[0083] where 0≦f′₁(x)≦∞, 0≦f′₂(x)≦∞, such that f′₁(x) and f′₂(x) are notboth zero; f′₂(x)=0, when the vehicle 10 is coincident with a surface ofthe obstacle 300, and where f′₁(x)=0, when the vehicle 10 is at point R.As with the previous example, both f′₁(x) and f′₂(x) are linear ornon-linear continuous functions with x being a parameterized constant,function, or family of functions used in the description of functionsf′₁(x) and f′₂(x), where D is the local distance from the obstacle orpoint to be avoided 300 along e_(int) to the geometric surface point oftangency R. Further, functions f′₁(x) and f′₂(x) should be constructedsuch that ∥E_(avoid)∥≠0.

[0084] Although not shown, after vehicle 10, which is inside geometricboundary 310, passes obstacle 300, which is also inside geometricboundary 310, the guidance command can be modified in a manner similarto that of eqns. (3A) and (3B). Once the vehicle 10 has passed theobstacle 300, the e′_(tan) vector can be blended with the edeairedvector by continuous linear or non-linear functions g₁(x) and g₂(x) toconstruct e_(tan)*. The vector e_(tan)* is constructed based on e′_(tan)and e_(desired) through function g(x) so that e_(tan)*→e′_(tan) when thevehicle 10 is approaching the obstacle 300 and e_(tan)*→e_(desired)after the vehicle passes the obstacle, and where e_(tan)* can bedescribed in accordance with eqns. (3a) and (3b). Like functions h₁(x)and h₂(x), functions g₁(x) and g₂(x) may be limited to operation over aspecified range, γ. As with the functions, h₁(x) and h₂(x), describedabove, it is preferable that functions g₁(x) and g₂(x) be the same asg₁(x) and g₂(x) for the case where the vehicle is outside the geometricboundary in order to avoid potential discontinuity at the geometricboundary surface.

[0085] When the functions g₁(x) and g₂(x) are used to modify thebehavior of the avoidance command as the vehicle 10, inside thegeometric boundary 310, passes the obstacle 300, e_(tan)* may besubstituted into equation (5b) to yield equations (6a) and (6b), shownbelow, which is a more robust solution for obtaining e_(avoid) and whichaddresses the desirable transition of the guidance command after thevehicle 10 passes the obstacle 300. In the above situation:

e _(avoid) =E _(avoid) /∥E _(avoid)∥  (6a)

[0086] where

E _(avoid) =f′ ₁(x)e _(desired) +f′ ₂(x)e _(tan)*  (6b)

[0087] where f′₁(x) and f′₂(x) are as defined above in regard toequations (5a) and (5b), and where e_(tan)* is determined as definedabove in regard to equations (3a) and (3b).

[0088] To reiterate, during application of the spatial avoidance method,as the vehicle 10 is approached by, or approaches an obstacle and isoutside of the geometric boundary, the direction e_(desired) will bemodified based on a function of the tangent of the geometric boundary(eqns. 1a, 1b, 2a and 2b) and based on a function of the approaching orretreating obstacle state (eqns. 3a, 3b, 4a and 4b). Thus, at theboundary surface, the vehicle will be directed to fly tangent to theboundary and consequently fly around the obstacle along the edge of theboundary surface. When the vehicle has sufficiently avoided theobstacle, as determined by eqns. (3a) and (3b), the avoidance command istransitioned from an avoidance trajectory to the desired trajectory.

[0089] However, if the vehicle 10 penetrates the boundary, the directione_(desired) will be modified based on a function of the tangent of thegeometric boundary (eqns. 1a, 1b, 5a and 5b), and based on a function ofthe approaching or retreating obstacle state (eqns. 3a, 3b, 6a and 6b),such that the vehicle will be directed away from the obstacle. Themethod is continuous at the boundary surface because when the vehicle isjust inside of the boundary, the vehicle is directed to fly along atangent of the boundary, therefore providing a continuous transitionthrough the boundary. The avoidance direction is transitioned from theboundary tangent, to a direction away from the boundary tangent, basedon (for example) the distance γ from the obstacle (eqns. 1a, 1b, 5a and5b). When the vehicle has sufficiently avoided the obstacle, theavoidance command is transitioned from an avoidance trajectory to thedesired trajectory (eqns. 3a, 3b, 6a and 6b).

[0090] These concepts of avoidance are not limited to the specificgeometric spaces (i.e., 3-dimensional space) as described previously.These avoidance concepts may be used to avoid acceleration, velocity, orother mathematically defined obstacles in N dimensional space. Theimplementation methodology will be the same. That is to say, theapplication of eqns. 1 through 6 would be the same, although theweighting functions associated with those equations would become morecomplicated. An implementation for such an algorithm can be in theavoidance of guidance algorithm singularities, whereby the resultingguidance command would be the zero vector. This construction can involvethe position, velocity and the acceleration of the vehicle as well asthe position, velocity and acceleration of the target.

[0091] For instance, one of ordinary skill in the art will appreciatethat a mathematically defined obstacle to be avoided can include asingularity in a vehicle primary control algorithm at which a definedresponse does not exist. Accordingly such singularity can bemathematically defined as an obstacle to be avoided by the avoidancealgorithm of the present invention. With this approach, the avoidancealgorithm will cause the vehicle to be maneuvered such that it does notreach a state where the primary control algorithm is at a singularity.

[0092] Another implementation could be in the construction of anavoidance algorithm to control the body orientation of a vehicle duringapproach to a destination so that the line of sight to the destinationis maintained to be not more than X degrees off the vehicle bore-sight(x-direction). In this instance the obstacle is not a material item buta mathematical construction. In addition, the boundary surface for thisconstruction is a cone emanating from some point relative to the vehiclealong the body x and having a cone angle of 2X. The avoidance algorithmwould be constructed using the same equations as before. However, thedefinitions of the weighting functions would be more involved.

SECOND EMBODIMENT (Operational Boundary Avoidance) EXAMPLE 1 MinimumAltitude

[0093] Application of the method and system of the present invention toavoidance of operational boundaries (rather than obstacles) is describednext as a second preferred embodiment. As will be explained furtherbelow, the basic approach is the same as outlined for the firstembodiment.

[0094] Operational boundaries, outside of which a vehicle travelsnormally, can be addressed with the method of the present invention asdescribed above. For instance, as illustrated in FIG. 5, if anoperational boundary for a vehicle is a minimum altitude, a sphere canbe used as a geometric boundary 310 having a center located at thecenter of the earth and having a radius suitable to place a surface ofthe sphere at the desired minimum altitude. Accordingly, by followingthe approach described above for avoidance of obstacles, the vehicle 10would avoid flying below the minimum altitude boundary 310 and hence notimpact the surface 300. In this situation, an oblate spheroid representsthe minimum altitude boundary 310 of the vehicle 10 and corresponds tothe obstacle 300 in the prior descriptions. As will be appreciated bythose skilled in the art, the geometric boundary 310 described in allexamples and embodiments herein is not limited to a sphere surroundingan object 300 and may be a continuous regular or irregular geometrywhich may be fluid in shape and as complicated as required.

[0095]FIG. 5 illustrates the avoidance guidance command e_(avoid) for avehicle 10 in each of positions A, B and C shown. In FIG. 5, Rrepresents the surface point of tangency on the minimum altitudeboundary surface and P represents an intercept point on the surface 300to be avoided. Vector e_(NI) represents normal vector to the surface tobe avoided at point P. The other vectors shown in FIG. 5 have the samemeaning as do the vectors by the same names shown in FIGS. 3A, 3B and 4.Equations used to solve for e_(avoid) for a vehicle 10 located atpositions A, B, and C are similar to those of eqns. (1a) through (6b)above.

EXAMPLE 2 Maximum Altitude

[0096] Another operational boundary which can be addressed by thepresent invention is a maximum altitude boundary. Like the minimumaltitude boundary situation illustrated in FIG. 5, the maximum altitudeboundary scenario illustrated in FIG. 6 uses an oblate spheroid torepresent the maximum ceiling 300, which corresponds to a surface of theobstacle 300 in the prior descriptions. A maximum ceiling geometricboundary 310 is defined a predetermined distance from the maximumceiling spheroid 300 and the approach described above for the FirstEmbodiment is used to determine avoidance guidance commands for thepresent conditions.

[0097]FIG. 6 illustrates the avoidance guidance command e_(avoid) for avehicle 10 in each of positions A, B and C shown. In FIG. 6, Rrepresents the surface point of tangency on the maximum altitudeboundary 310 and P represents an intercept point on the surface 300 tobe avoided. Vector e_(NI) represents normal vector to the surface 300 tobe avoided point P. The other vectors shown in FIG. 6 have the samemeaning as the vectors by the same names shown in FIGS. 3A, 3B and 4.Equations used to solve for e_(avoid) for a vehicle 10 at positions A, Band C are similar to those of eqns. (1a) through (6b) above.

EXAMPLE 3 Use for Navigation of a Submerged Vehicle

[0098] While the geometric boundary surfaces have been illustrated thusfar as surfaces of simple geometry, and while the exemplary vehiclesdescribed thus far have been aircraft or missiles, the method of thepresent invention is equally applicable to other types ofimplementations. For instance, the method can be applied to thenavigation of a submarine while traveling submerged and can be appliedto boats and shipping vessels as well.

[0099]FIG. 7 illustrates an irregularly shaped geometric boundarysurface 310 as may be applied in an application for obstacle avoidancefor a submarine 10 application. The actual terrain to be avoided can besimulated by a smoothed surface 300 to simplify the topography of thegeometric boundary surface 310. Because the relevant guidance vectorsand determination of avoidance guidance command e_(avoid) are the sameas illustrated and described in regard to the prior examples, FIG. 7omits these details for brevity and clarity. However, equations used tosolve for e_(avoid) in this application are similar to those of eqns.(1a) through (6b) above.

EXAMPLE 4 Avoidance of Moving Obstacles:

[0100] While the above example situations illustrate obstacles which avehicle must avoid, the present method is equally applicable toavoidance of active threats or moving obstacles. In the case of anapproaching threat, if the position of the threat is known, a geometricboundary can be defined around the threat position and the geometricboundary position of the threat can be continually updated. A vehiclewhich is being guided in accordance with the present method will thusautomatically be directed to avoid the boundary and thus the threatposition even as the obstacle or threat moves in relation to the movingvehicle.

[0101] Avoidance of a threat is illustrated in FIG. 8 showing a movingvehicle 10 having a geometric boundary 310 moving with the vehicle 10,and a moving obstacle to be avoided, a missile 300. As illustrated inFIG. 8, the avoidance commands change, due to use of the present method,based on the relative position between the moving vehicle 10 and themoving threat missile 300. Specifically at threat positions t1, t2, t3,t4, t5 and t6, the corresponding avoidance guidance commands are shownas a1, a2, a3, a4, a5 and a6. In the case corresponding to threatposition t1, where the missile 300 is outside the geometric boundary310, the commands are blended between the desired flight pathe_(desired) and the tangent based avoidance path e′_(tan). In the casecorresponding to threat positions t2-t4, where the missile is outsidethe geometric boundary but closer than position t1, the weighting of thetangent based avoidance direction is increased. In the casecorresponding to threat position t5, where the missile is at the surfacepoint of tangency, R, the avoidance command a5 is based on the geometricboundary tangent avoidance command e′_(tan). In the case correspondingto threat position t6, where the missile is inside of the geometricboundary 310, the avoidance command is weighted between ananti-intercept command and a tangent based command as described in eqns.(5a) and (5b) above for the First Embodiment. Thus, e′_(tan) and thefunctions used to calculate it must be reevaluated at each point alongthe flight path to continually update the steering command, e_(avoid).Because the relevant guidance vectors and determination of avoidanceguidance command e_(avoid) are the same as illustrated and described inregard to the prior examples, FIG. 8 omits these details for brevity andclarity. However, equations used to solve for e_(avoid) in thisapplication are similar to those of eqns. (1a) through (6b) above.

EXAMPLE 5 Avoiding Moving Object Launched from Moving Vehicle

[0102] As noted in the background of the invention, one shortcoming withthe conventional avoidance technique based on use of geometric boundaryshapes occurs in the situation of launching a missile from an aircraft.Specifically, because the conventional boundary shape method requiresthat the geometric shape be made very small so that the vehicle and themissile are not both inside the geometric shape, the resulting geometricshape would be smaller than the vehicle to be avoided and the missilewould thus be able to fly into the launching aircraft. As illustrated inFIG. 9A, at the time of launch, a missile 410 and launching aircraft420, both within the geometric shape 310, have the same velocity anddirection vector. The desired missile guidance command, however, wouldpotentially cause the missile 410 to pull up upon launch and strike theaircraft 420.

[0103] In contrast, the method according to present invention, is ableto provide avoidance guidance commands which will cause a launchedmissile 410 to avoid the aircraft 420 from which it is launched. Asillustrated in FIG. 9B, a launched missile 410 would first maneuveroutside of the geometric boundary 310 which surrounds the aircraft 420and then turn onto the desired guidance course. More specifically, FIG.9B shows the missile guidance states at four points during the launchand flight sequence, P1, P2, P3, and P4. At positions P1 and P2, whichcorrespond to the situation where the missile 410 is inside thegeometric boundary 310, the boundary tangent vector and a vector havinga direction opposite to the center of the aircraft are blended togetherto obtain the avoidance command as described above with reference to theFirst Embodiment. (In this example, the center of the aircraft isdefined as a reference point on the aircraft that represents the wholeaircraft). At position P3, which corresponds to the situation where themissile 410 is at the surface of the geometric boundary 310, theavoidance command is based on the geometric boundary tangent avoidancevector. At position P4, which corresponds to the situation where themissile 410 is outside of the geometric boundary 310, the avoidancecommand is obtained by blending together the geometric boundary tangentcommand e′_(tan), and the desired missile guidance command, precisely asshown and illustrated in FIG. 3A and as described in the textaccompanying FIG. 3A. Thus, e′_(tan) and the functions used to calculateit must be reevaluated at each point along the flight path tocontinually update the steering command, e_(avoid). Because the relevantguidance vectors and determination of avoidance guidance commande_(avoid) are the same as illustrated and described in regard to theprior examples, FIG. 9B omits these details for brevity and clarity.However, equations used to solve for e_(avoid) in this application aresimilar to those of eqns. (1a) through (6b) above.

THIRD EMBODIMENT (Control of Vehicles in Formation)

[0104] The method by which the present invention may be used for controlof vehicles traveling in formation is described next as a thirdpreferred embodiment. This embodiment builds on previous embodimentswith the addition of sink elements for position, velocity and otherstate control.

[0105] As previously noted, an object to be avoided, and/or thesurrounding region, can be referred to as a source element, whileobjects or regions towards which the vehicle is attracted can bereferred to as sink elements.

EXAMPLE 1 Geometric Boundary Formed from Source and Sink Elements

[0106] As indicated above, the method of the present invention may beused to maintain control of vehicles in formation and to maintain theposition and separation of the vehicles in that formation. One approachin accordance with the present method is to replace the source elementsinside of a geometric boundary with sink elements in combination withpreviously mentioned avoidance boundaries. Because a vehicle isattracted to sink elements, the sink elements will thus guide thevehicle to a particular location or guide the vehicle to a particularspeed and location, and keep the vehicle from colliding with othervehicles in the formation.

[0107]FIGS. 10A and 10B illustrate the situation where two aircraft 420Aand 420B are operating in a formation. In one approach, lead aircraft420A provides data to trailing aircraft 420B which defines for trailingaircraft 420B acceptable positions of formation flight. This provideddata defines for trailing aircraft 420B geometric sink elements 520.FIG. 10A is a side view of the aircraft 420A and 420B in formation. Alsoshown are boundaries 510 around each aircraft 420A and 420B,respectively, which is the limit of extent of the source potential fieldwhich can be defined. Boundary 540 represents a limit of the extent ofthe source tangent field. Boundary 530 represents a limit of the extentof the sink potentials, and the sink potentials have no geometric basedavoidance vector outside of this boundary. Inside this boundary, thestrength of sink 520 is defined by functions k₁(x) and k₂(x), which areconstructed similar to f₂(x) and f′₂(x), where the definition of L isnow a surface. Thus sink 520 will only attract objects which are withinthe limit of boundary 530. The boundaries 510 will operate as geometricboundaries 310 described in the First and Second Embodiments, therebypreventing collision of the aircraft 420A, 420B while sinks 520 guidethe aircraft 420A, 420B to predefined relative positions.

[0108] The mechanism by which the effects of sinks 520 are added to thespatial avoidance guidance equations is as follows: Inside the boundary530, the vector e_(attract-sink) is constructed such that

E _(attract-sink) =k ₁(x)e _(sinkpnt) +k ₂(x)e _(desired)  (7a)

[0109] where e_(sinkpnt) is a vector in the direction from the vehicleto a center of a sink; where 0≦k₁(x)≦∞ and 0≦k₂(x)≦∞, such that k₁(x)and k₂(x) are not both zero; and where k₁(x)=0, when the vehicle 10 iscoincident with surface L (not shown), and k₂(x)=0, when the vehicle 10is coincident with the sink 520. Both k₁(x) and k₂(x) are linear ornon-linear continuous functions with x being a parameterized constant,function, or family of functions as described above. As above, functionsk₁(x) and k₂(x) should be constructed such that ∥E_(attract sink)∥≠0.

[0110] The vector e_(attract-sink) is calculated in equation 7b based onE_(attract-sink) computed in equation 7a:

e _(attract-sink) =E _(attract-sink) /∥E _(attract-sink)∥  (7b)

[0111] As will be appreciated by those skilled in the art, thee_(attract-sink) directional vector is then mixed with the e_(avoid)command, per eqns. 8a and 8b below, to get e_(avoid-final), thuspreventing collisions.

e _(avoid-final) =E _(avoid-final) /∥E _(avoid-final)∥  (8a)

E _(avoid-final) =z ₁(x)e _(desired) +z ₂(x)e _(tan)  (8b)

[0112] where 0≦z₁(x)≦∞ and 0≦z₂(x)≦∞, such that z₁(x) and z₂(x) are notboth zero; and where z₁(x)=0, when the vehicle 10 is coincident withsurface L (not shown), and z₂(x)=0, when the vehicle 10 is coincidentwith the sink 520. Both z₁(x) and z₂(x) are linear or non-linearcontinuous functions with x being a parameterized constant, function, orfamily of functions as described above. As above, functions z₁(x) andz₂(x) should be constructed such that ∥E_(avoid-final)∥≠0.

[0113] The e_(attract-sink) directional vector then becomes thee_(desired) vector of the first two examples. Hence, as the followingvehicle approaches the leading vehicle, it will be directed toward thesink locations and away from the leading vehicle. As the followingvehicle begins to slow or recede, depending on the definition of thetransition zone, it will begin to ignore the avoidance commands and bedirected toward the sink locations. The transition zone definitionbecomes important in this discussion.

[0114] For formation maneuvering, such as flying, the transition zonecan be dynamically varied and/or its influence dynamically increased ordecreased over time, space, or based on the condition of the vehicles,to more specifically tailor the effect on the vehicles. Specifically,for instance, in the situation where aircraft are joining a formation,the transition zone can be brought into existence only when certaincondition(s) are satisfied, such as when the rate of closure between thevehicles is below a specified threshold. Similarly, the transition zonecan be removed when one aircraft begins to recede from another. Withsuch an approach, if an aircraft is approaching another aircraft at toogreat a speed, the transition zone, and hence the effect of theattracting sink, does not influence the guidance of the vehicle, butinstead the avoidance algorithm, unaffected by the sink, will commandthe vehicle to maneuver so as to avoid a collision. However, once theavoidance algorithm-based control commands have moved the vehicles intoa safe separation distance, the transition zone will be created and thee_(attract-sink) vector-based effect will head the vehicle towards thedesired position for formation flying. Therefore, even if the vehiclesare approaching each other with too much speed, the avoidance algorithmwill cause them to avoid each other. When the vehicles start to moveapart and have reached a safe distance, the e_(attract-sink) vector willonce again be the desired direction toward which the vehicle will fly.

[0115] Note the implementation described above, and shown in FIGS. 10Aand 10B, differs from the previously described embodiments in that nogeometric based avoidance commands are generated by the sink elements.Specifically, the geometric-based commands developed through theaircraft avoidance, using 420 and 510, are blended with thenon-geometric-based attracting commands to define the final formationflying guidance command.

[0116]FIG. 10A also shows the formation avoidance commands which aregenerated for aircraft 420B at each of six relative positions betweenaircraft 420A and 420B. Specifically, FIG. 10A shows six arrowsrepresenting the formation avoidance command that aircraft 420B wouldreceive if aircraft 420B were located at the corresponding formationposition f1, f2, f3, f4, f5 and f6, respectively. As trailing aircraft420B approaches leading aircraft 420A, the corresponding avoidancecommands steer aircraft 420B to the sink 520. If aircraft 420B closelyapproaches aircraft 420A so that it comes within the range of the sourcefield 510 surrounding aircraft 420A, then aircraft 420B would receive afurther guidance command so as to avoid a collision.

[0117] Aircraft 420A and 420B can break formation either by overridingthe above described guidance method, or can break formation while stillunder such guidance method by turning off the sinks 520. In the absenceof the attracting force from the sinks 520, the aircraft 420A and 420Bwould separate.

[0118] It will be appreciated that the size and location of theboundaries and the sinks must be tailored to achieve the desired spacingand relative positions of the vehicles in a given formation. Because thepresent invention is not limited to a fixed size or shape boundary, thesize and shape of the boundaries can be adjusted to achieve theobjectives of a particular situation. For instance, increasing thegeometric boundary size results in avoidance commands which are able toact at a further distance from a vehicle. On the other hand, decreasingthe boundary size will allow a second vehicle (or threat) to move closerto a first vehicle before avoidance guidance commands would begenerated.

[0119] Moreover, for formation flying, the size of the boundaries may bedynamically altered based on the relative velocity between the vehicles.For instance, when the relative velocity between vehicles is nearlyzero, the size of the boundaries may be reduced to a minimum safe size.However, when the relative velocity between the vehicles is large, thesize of the boundaries can be increased to allow collision avoidanceprotection within the maneuverability limits of the vehicles.

[0120]FIGS. 10A and 10B also show that a given situation can includemultiple sinks 520 and/or sources 510 within a single boundary 530. Thepresent invention can be applied with an arbitrary number of sinkpotential elements each representing individual points, or many sinkpotentials may collectively define a sink surface or curve.

EXAMPLE 2 Dual Zone Combined Source and Sink Geometric Boundary

[0121] Another possible method for obstacle avoidance when flying in aformation is shown in FIG. 10C. A multi-zone method having a sourcegeometric boundary 510 emanates from a point and surrounds vehicle 420Athereby defining a first zone 510. A sink geometric boundary 520emanates from the same point on vehicle 420A and surrounds sourcegeometric boundary 510 thereby creating a second zone or interactiveregion 525 between the source geometric boundary 510 and the sinkgeometric boundary 520. Within the source geometric boundary 510, theavoidance vector is computed as shown in the First Embodiment. Beyondthe sink geometric boundary 520, the avoidance vector is computed asdisclosed above in the Second Embodiment. Within the interactive region525 between source geometric boundary 510 and sink geometric boundary520, the avoidance vector is computed as a combination of the repellingforce of the source geometric boundary and the attracting force of thesink potential of the sink geometric boundary. A resulting relativeavoidance vector may thereby be computed for each point within theinteractive region 525.

[0122] The spatial guidance method of Example 2 above operates such thatwhen vehicle 420B is flying outside the dual geometric boundaries 510,520, vehicle 420B is attracted to sink boundary 520 and the avoidancevector is directed toward vehicle 420A. When vehicle 420B is within theinteractive region 525, the relative avoidance vector directs vehicle420B to maintain a position within that region 525 based on weightedfunctions. As vehicle 420B approaches source geometric boundary 510, therepelling component of the relative avoidance vector is increased orweighted in favor of the direction away from vehicle 420A, therebyeffectively directing vehicle 420B away from vehicle 420A. As vehicle420B moves away from source geometric boundary 510, the attractingcomponent of the relative avoidance vector is increased or weighted infavor of the direction toward vehicle 420A, thereby effectivelydirecting vehicle 420B toward vehicle 420A.

[0123] The benefit of this approach is that solutions need be computedfor only one source and one sink. Further, this approach allows onevehicle in formation to move independently of another in all directionswithin the interactive region surrounding one of the vehicles. Further,depending upon the requirements, the boundaries of the source and sinkmay be formed in any size and shape. Equations for calculating theavoidance vector within the interactive region are similar to equations(1a) through (8B) above. As one of ordinary skill in the art willappreciate, the equations above would be applied in the same manner,adjusted for the particular geometry of the embodiment.

[0124] In a modification of this approach shown in FIGS. 10D, allvehicles in formation could have dual source and sink geometricboundaries, thereby providing a region surrounding each vehicle forformation flight and affording more complete vector avoidance controlduring formation flight. As shown, a sink overlap region 526 formsbetween sink geometric boundaries of vehicles flying within a relativelyclose proximity to a boundary region of another vehicle. By this method,it is possible to calculate the appropriate steering vector for aplurality of vehicles and to maintain a desired separation distancebetween them during formation flight. As one of ordinary skill in theart will appreciate, the equations above would be applied in the samemanner, adjusted for the particular geometry of the embodiment.Equations for calculating the interrelated avoidance vectors within theinterrelated interactive regions of multiple vehicles are significantlymore complicated than with the simple prior examples. However, theseequations are functionally similar.

Exemplary Field Diagrams Generated by Using the Method for MultipleSource and Sink Conditions

[0125] The method of the present invention and the results of theapplication of the present invention are further explained by vectorfield diagrams in FIGS. 11-19. These vector field diagrams aresimulation results from the application of the method of the presentinvention as applied to several situations comprising stationaryimpediments and/or stationary vehicles in a two dimensional space. Thereason for applying the present invention to a two dimensional space andto stationary impediments and/or stationary vehicles in the followingdiscussion is for simplicity of explanation only. Those of skill in thepresent art will appreciate that the present invention can be appliedwith equal ease for dimensions above two and for both moving impedimentsand vehicles, however, static figures representing such dynamicsituations are more difficult to render. Additionally, while thegeometric boundaries shown in FIGS. 11-19 consist of circles and planes,this is not a limitation of the present invention. In practice, theboundaries can be represented by any smooth function.

[0126] FIGS. 11-19 were constructed using weighting functions of thefollowing form which also illustrate the intent of the general functiondescriptions of eqns. 1a though 6b discussed in the First Embodiment.However, for clarity and brevity when discussing specific examples ofthe present method, the following discussion will be limited andspecifically directed to that of the First Embodiment. It will beunderstood that the method could be similarly applied to and similarresults computed for all embodiments discussed herein.

[0127] Referring now to FIGS. 11-19, the equations used to calculate thevector diagrams include:

E′ _(tan) =h ₁(x)e _(N) +h ₂(x)e _(tan)  (A)

[0128] where

[0129] h₁(x)=0.0, and

[0130] h₂(x)=1.0.

[0131] Further, equations include:

E _(avoid) =f ₁(x)e _(desired) +f ₂(x)e′ _(tan)  (B)

[0132] where when e_(desired)·e_(r0)<0,

[0133] f₁(x)=x where r_(dist)/r₀, and

[0134]f₂(x)=−x*(e_(desired)·e_(tan))+sqrt(1−x²*(1−(e_(desired)·e_(tan))²))

[0135] where r_(dist) is the distance from the current point to thenearest point on the obstacle, and r₀ is the distance from the currentpoint to the obstacle definition point.

[0136] A result of the above f₁(x) and f₂(x) construction is thatE_(avoid) is now a unit vector having direction and unit magnitude andhence:

[0137] e_(avoid)=E_(avoid).

[0138] Further, as computed according to the equations of the FirstEmbodiment:

E* _(tan) =g ₁(x,γ)e _(desired) +g ₂(x,γ)e′ _(tan),  (C)

[0139] where the functions g₁(x, γ) and g₂(x, γ) were implemented asshown in the following. In the equations below, two principle situationsmust be considered:

[0140] (A) where (e_(desired)·e_(r0))<0 and

[0141] (B) where (−e_(rt0)·e_(r0))≦0. In the following equations:

[0142] x is defined above as (x=r_(dist)/r₀)

[0143] γ is a boundary defined by the following conditional statements.

[0144] In the first situation (A) where, (e_(desired)*e_(r0))<0, twosub-cases must be considered:

[0145] (1) When: (−e_(rt0)·e_(r0))>0 and (−e_(rt0)*e_(desired))>0 then$\begin{matrix}{{{g_{2}\left( {x,\mathrm{\Upsilon}} \right)} = \quad \left( {1 + {\left( {e_{desired} \cdot e_{r0}} \right)\left( {{- e_{rt0}} \cdot e_{r0}} \right)\left( {{- e_{rt0}} \cdot e_{desired}} \right)}} \right)},{with}} \\{{g_{1}\left( {x,\mathrm{\Upsilon}} \right)} = \quad {{{- {g_{2}\left( {x,\mathrm{\Upsilon}} \right)}}*\left( {e_{\tan}^{\prime} \cdot e_{desired}} \right)} + {{sqrt}\left( 1 \right.} - {{g_{2}\left( {x,\mathrm{\Upsilon}} \right)}^{2}*}}} \\{{\quad \left. \left( {1 - \left( {e_{\tan}^{\prime} \cdot e_{desired}} \right)^{2}} \right) \right)},{{{when}\quad {e_{\tan}^{\prime} \cdot e_{desired}}} > 0},{and}} \\{{{g_{1}\left( {x,\mathrm{\Upsilon}} \right)} = \quad {{sqrt}\left( {1 - {g_{2}\left( {x,y} \right)}^{2}} \right)}},}\end{matrix}$

[0146] when e′_(tan)*e_(desired)≦0.

[0147] (2) Otherwise, when (−e_(rt0)·e_(r0))≦0 or when(−e_(rt0)·e_(desired))≦0

[0148] g₁(x, γ)=0.0

[0149] g₂(x, γ)=1.0.

[0150] In the second situation (B) where, (e_(desired)·e_(r0))<0, twocases must be considered:

[0151] (1) when: (e_(desired)·e_(r0))<1

[0152] g₂(x, γ)=(1−(e_(desired)·e_(r0)))*x

[0153] g₁(x, γ)=−g₂(x, γ)*(e′_(tan)·e_(desired))+sqrt (1−g₂(x,γ)²*(1−(e′_(tan)·e_(desired))²)).

[0154] (2) otherwise when: (e_(desired)·e_(tan))≧1

[0155] g₁(x, γ)=1.0

[0156] g₂(x, γ)=0.0.

[0157] In each of the above, e_(r0) is the unit vector from the currentpoint to the obstacle definition point, e_(rt0) is the unit vector fromthe destination to the nearest point on the obstacle, and where x is acontinuous function. The symbol “·” represents the vector dot productfunction.

[0158] The above example illustrates the types of blending functionsused in the development of the examples, and does not represent specificalgorithms required to implement the method of the invention.

[0159] As shown in FIGS. 11-19, each point in the field represents apossible location of a vehicle, and the small line coming from eachpoint indicates the direction of the avoidance guidance command(e_(avoid)) which the vehicle would receive if located at that point. Ineach of FIGS. 11-19, the nominal desired guidance command from any oneof the points in the vector field is a straight line from that point tothe desired destination.

[0160] Specifically, FIG. 11 represents a situation where only oneobstacle 300 is present. As noted above, the nominal desired guidancecommand 611 is a vector originating from any one of the points 612 inthe vector field is a straight line in a direction from that point 612to the desired destination 610. As can be seen in FIG. 11, the directionof the guidance command 611, as indicated by the direction of the smalllines which radiate from the location points 612, is a function of thelocation of the point within the field relative to both the obstacle 610and the destination. For points 612 which are far from the obstacle 300,such as points 612 in the upper right hand corner of the field, thedirection of the guidance command 611 is nearly straight towards thedestination 610 and is essentially unaffected by the obstacle 300.

[0161] In contrast, points 612 which are behind the obstacle 300,relative to the destination 610, indicate a direction of the guidancecommand 611 which will steer a vehicle (not shown) smoothly around theobstacle 300. It should be noted the degree to which the guidancecommand 611 differs from the nominal desired direction varies as afunction of the distance from the point 612 to the boundary 620. This isbecause the guidance command 611 is determined by blending together thenominal desired direction and e_(int) in varying proportion depending ondistance from the obstacle 300.

[0162] It should also be noted that the direction of the guidancecommand 611 for each point smoothly transitions from the interior ofboundary 620 to the exterior of boundary 620. Thus, vehicles can besmoothly controlled even in the case where obstacle geometric boundaries620 are crossed.

[0163]FIG. 12 represents a situation where multiple obstacles arepresent in the vector field. While only one obstacle is shown with areference number 300 in FIG. 12, each of the three black dots which aresurrounded by a circle are obstacles 300, the surrounding circle of eachobstacle 300 is its corresponding boundary 620. In the case where morethan one obstacle 300 is present, an avoidance direction for eachobstacle is calculated for each of the positions or points 612 in thevector field. Then, a weighting function is used, for each position 612,to prioritize the guidance commands calculated based on each of theseveral obstacles 300. By weighting the contribution of each calculatedguidance command and blending them together to produce a singlenormalized guidance command 611, the influence of multiple obstacles 300can be simultaneously addressed.

[0164] In weighting the calculated guidance commands, two operationalzones were defined and separate weighting functions were defined forapplication in each zone. The first operational zone is inside aboundary and the second operational zone is outside a boundary.

[0165] In the case where the point is inside the boundary, weightingfunction (10a) below is used. In the case where the point is outside ofthe boundary, weighting function (11a) is used.

W _(I) =M/(%_(RI))  (10a)

W _(O)=1/(%_(RI) ·R ² ·MAX(%_(RO) , M/(%_(RI))))  (11a)

[0166] where

[0167] (%_(RI))<1 inside the geometric boundary, and

[0168] (%_(RI))>1 outside the geometric boundary.

[0169] As can be seen from equation (10a) above, in the case of a pointwhich is inside the boundary, for the avoidance vector calculated fromeach of the obstacles, the avoidance vector is divided by the percent ofintrusion into the geometric boundary (%_(RI)) along e_(int) andmultiplied by M, the number of obstacles in the space. To determine thecomposite (or final) avoidance command, all of the scaled avoidancecommands are summed and the result normalized to produce the finalavoidance commands as shown by the guidance command vectors in FIG. 12.Thus, determination of the final avoidance command can be expressed asshown below in equation (10b). $\begin{matrix}{e_{avoid} = \frac{\left( {\sum{e_{{avoid}{(n)}} \cdot W_{I{(n)}}}} \right)}{{\sum{e_{{avoid}{(n)}} \cdot W_{I{(n)}}}}}} & \text{(10b)}\end{matrix}$

[0170] where n is the number 1 through M of the point being consideredin the weighting function.

[0171] As can be seen from equation (11a) above, in the case of a pointwhich is outside the boundary, for the avoidance vector calculated fromeach of the obstacles, the avoidance vector is weighted by a non-linearfunction which is a function of the percent distance outside of theboundary relative to the local geometric boundary radius (%_(RO)), thelocal geometric boundary radius R, and W_(I).

[0172] As with points inside the boundary, to determine the composite(or final) avoidance command, all of the scaled avoidance commands aresummed and the result normalized to produce the final avoidance command.Thus, determination of the final avoidance command can be expressed asshown below in equation (11b). $\begin{matrix}{e_{avoid} = \frac{\left( {\sum{e_{{avoid}{(n)}} \cdot W_{O{(n)}}}} \right)}{{\sum{e_{{avoid}{(n)}} \cdot W_{O{(n)}}}}}} & \text{(11b)}\end{matrix}$

[0173] It should be noted that the weighting function used to weight theavoidance commands calculated from each of the obstacles is not limitedto functions (10a) and (11a) as described above and other weightingfunctions can be used.

[0174] The guidance command vectors shown in FIGS. 13-19 were determinedbased on the same weighting approach described above for FIG. 12.

[0175]FIG. 13 represents a situation where multiple obstacles arepresent in the vector field and where the boundaries 620 of theobstacles overlap. While only one obstacle 300 has a reference number inFIG. 12, each of the three black dots which are surrounded by a circleare obstacles 300, the surrounding circle of each obstacle is itscorresponding boundary 620. The guidance command vectors shown in FIG.13 were determined based on the same weighting approach described abovefor FIG. 12.

[0176]FIG. 14 represents a situation which is essentially similar tothat shown in FIG. 13 but where the obstacle is modeled as a curve orsurface composed of an infinite number of source elements. Thecalculation of %_(RI) is based on the length from the boundary surface620 to the source surface 613 along e_(int) rather than from theboundary surface 620 to the center of the impediment location 300.

[0177]FIG. 15 represents a situation which is essentially similar tothat shown in FIG. 14 but where the destination 610 is inside ageometric boundary 620. It should be noted here that the guidancecommand vectors 611, as shown, will steer a vehicle into a geometricboundary 620 to reach the destination while avoiding the surface of thesource potentials 613.

[0178]FIG. 16 represents a situation which is essentially similar tothat shown in FIG. 14 but where one of the boundaries 620 is anoperational boundary 620A such as a maximum ceiling and is thus anon-closed infinite impediment in the avoidance vector solution.

[0179]FIG. 17 represents a situation which is essentially similar tothat shown in FIG. 16 and which demonstrates an additional aspect of thepresent invention. Specifically, in the prior discussions, the tangentvector to the geometric boundary was always described as being in thedirection of the desired guidance command. In some situations such ascontrolled traffic patterns, it is desirable to use the tangent vectorto the geometric boundary in a direction which is independent from thedesired guidance command. Such a use of the tangent control isapplicable to the management of shipping lanes or other traffic patternswhere there are rules with respect to which size vehicles should beavoided. Accordingly, in FIG. 17, one of the obstacles 300B has beendefined to direct the current point in a direction clockwise around theobstacle; another of the obstacles 300A has been defined to direct thecurrent point in a direction counter-clockwise around the obstacle.

[0180]FIG. 18 represents a situation which is essentially similar tothat shown in FIG. 17 which demonstrates an additional aspect of thepresent invention. Specifically, one of the source elements has beensubstituted by a sink curve element 614. As discussed above, such anelement has the effect of attracting a vehicle to its location. Thistype of arrangement can be applied in logistics planning models where acollection area is defined for accumulation of vehicles at a location.Once the vehicles have been collected at that location, the sink element614 could be dissipated to permit the accumulated group to progressthrough the remainder of the space to the final destination 610. FIG. 18also illustrates aspects of formation flying as discussed with referenceto the Third Embodiment.

[0181]FIG. 19 represents a situation applicable in traffic management.In this arrangement, sink elements 614 are defined so as to move along adefined corridor 701 in the direction of arrow A. Because vehicles areattracted to the sinks 614, and because the sinks 614 are controlled tomove along the defined corridor 701 in a controlled manner, the vehicleswill automatically join and follow the controlled travel of the sinks614. As shown in FIG. 19, the corridor 701 has boundaries 700 which tendto confine to the corridor 701 vehicles which have not yet moved intoposition at a sink 614. Also as shown, the boundary 720 of each sink 614can be sized in accordance with the size of the corridor 701.

FOURTH EMBODIMENT (6 Degree of Freedom (6-DOF) Control where BothVehicle and Object are within a Geometric Boundary)

[0182] Use of the method of the present invention for control ofvehicles in 6DOF is described next as a Fourth Embodiment.

[0183] As shown in FIGS. 14-19, obstacles 300 are contained within, orpositioned behind, geometric boundaries 620. However, according to theFourth Embodiment of the present invention shown in FIGS. 20 and 21,both the vehicle to be controlled and the obstacles are contained withingeometric boundary surfaces which can be used to determine the controlrequired for vehicles and objects operating in 6-DOF.

[0184] As will be described next, in the case where both the vehicle 810to be controlled and the obstacles 800 to be avoided are containedwithin geometric boundary surfaces 811 a, 811 b, an interaction betweenthe surfaces within a field of influence 813 will produce a moment aboutthe guided vehicle. This moment can be used to extend the guidanceapproach and method.

[0185] More specifically, the approaches and methods described thus farfor controlling missile systems can provide directional guidance in only3 Degrees-of-Freedom (3-DOF) (i.e., translations along the directions x,y, and z axes). However, in the situation where a source potential curveor surface 811 b is used to represent the guided vehicle 810 and asource potential curve or surface 811 a is also used to represent anobstacle 800, an extension of the guidance commands 611 to 6-DOF (i.e.,both translations along and rotations about each of the directions x, y,and z) can be achieved. Specifically, the 6-DOF method, however,requires additional state information than the previous 3-DOFembodiments. The previous embodiments do not require attitudeinformation, whereas 6-DOF control requires knowledge of the vehicle 810attitude along with other state information. Further, additionalimplementations of the 6-DOF guidance system can be generated if threat,obstacle, or target attitude information is also provided.

[0186] Specifically, the effect can be constructed by using the vectore_(int) and the distances to some or all of the points defined byvectors which are parallel to e_(int) that intersect both the obstaclesource potential curve or surface 811 a and the vehicle source curve orsurface 811 b. At each of the intersection points on the guided vehiclesource curve 811 b, a weighting function is applied to average normaland average tangent vectors of the obstacle source potential curve 811a. The weighting function is based on the corresponding distance to theobstacle source potential curve 811 a within the field of influence 813.Through this approach, a distributed repulsive force can be constructedalong the guided vehicle source potential curve 811 b. The distributedforce that can be applied about a point 812 within the guided vehiclesource potential curve 811 b will produce a moment 817 about that point812 through which the commands of the vehicle may be tied. In athree-dimensional case, the moments would provide the vehicle withrotational guidance commands. These rotational guidance commands can becoupled with typical translational guidance commands to provide complete6-DOF guidance commands.

[0187] Such 6-DOF control systems can benefit systems such as a missilein an end game condition where a warhead, which has an effectivedirection, is used for intercept of the target.

[0188] Other potential applications include ship docking and spacecraftrendezvous and docking, which require management of thrusters toaccomplish precise docking maneuvers.

[0189]FIG. 20 also illustrates the vector field as shown in FIG. 14,however, at each point the vehicle 810 now possesses an effectiveattitude boundary surface (shown at one location only fordemonstration). The vehicle effective attitude boundary surface also hasa center of rotation associated with it, which is tied to the vehicleorientation, such that:

e _(vehicle) =Z(x)e _(effsurface)

[0190] where Z(x) is a real valued constant or a continuous linear ornon-linear function, and e_(effsurface) is the effective attitudeboundary surface of the vehicle at a single point on the vehicle and isa function of the vehicle rotation. The center of rotation is notlimited to the center of gravity or to the principle axes of thevehicle.

[0191] Inclusion of vehicle attitude will increase the guidance degreesof freedom from the traditional 3-DOF to 6-DOF, with 3 translational and3 rotational degrees of freedom. Achieving the rotational degrees offreedom requires that the vehicle 810 be affected by the avoidancealgorithm. This can be achieved through construction of an apparentfield between the obstacle(s) and the vehicle such that effectivemoments are placed on the guidance algorithm which in turn will producea desired vehicle attitude command (function Z(x), described in aprevious paragraph).

[0192] The desired attitude command may be produced by integrating adistance based function over the effective vehicle surface. Constructionof the field or distance based function may be described as follows (seeFIG. 21):$T = {{K(x)}{\int_{L}{\int_{L^{\prime}{(r)}}{\frac{\left( {r - r_{p}} \right)\left( {e_{Nv}*e_{Nobs}} \right)}{{r - r_{obs}}}{S_{obs}}{{Sv}}}}}}$

[0193] where T is the resulting attitude correction vector; K(x) is again scaling function to regulate the magnitude of the command; r is alocal point on the vehicle 10 effective surface Sν (surface denoted byν); r_(obs) is a local point on the obstacle surface; r_(p) is thecenter of rotation about which the vehicle orientation will becontrolled; e_(Nv) is the unit vector normal to the effective surface Sνat the point r; e_(obs) is the unit vector normal to the obstaclesurface S_(obs) at the point r_(obs); L are the limits of integrationwhich define the part of the effective surface which point in thedirection of the obstacle surface S_(obs) (denoted by obs); and L′ (r)are the limits of integration which define the part of the obstaclesurface which is visible from the current point r.

[0194] The resulting vector T will yield attitude commands about thepoint r_(p). The magnitude of the commands may be modified by the gainfunction K(x) to yield commands which will provide a stable attitudeguidance solution. The gain function K(x) may also be a spatially basedfunction, a time based function, or a function of spatial rates(velocity, acceleration, etc), whereby function K(x) changes magnitudebased on a given state. Variation of guidance gain magnitude is typicalfor guidance implementations of this type.

[0195] There are various ways to implement the function described above.The Fourth Embodiment described here addresses the use of the boundaryrepresentations to control more degrees-of-freedom than in typical priorart methods.

[0196] In addition to providing a means for 6-DOF guidance solutions,the Fourth Embodiment can be expanded beyond the six degrees-of-freedomguidance to include N degrees of guidance by surrounding points orsurfaces of interest with boundaries, and by then applying the avoidancemethods described in the First and Second Embodiments along with themethod of the Fourth Embodiment.

[0197] While the various embodiments of the present invention have thusfar been described as a vehicle control or guidance system, it will beappreciated by those of skill in the art that the method of the presentinvention can be implemented in hardware and/or software as part or allof a vehicle guidance control system.

Guidance Control Systems Implementing the Spatial Avoidance Method

[0198] Examples of vehicle guidance control systems implementing theabove described spatial avoidance method are illustrated in FIGS. 22-24.

[0199] Autonomous Vehicle Guidance Processor

[0200]FIG. 22 is an example of a configuration for a guidance processingunit 910, for an autonomous vehicle guidance processor. The processingunit 910 of FIG. 22 includes a navigation function 912 and a guidancefunction 914, and may also include an autopilot function 916 as well asat least one processor 918 for operation of the unit. The guidanceprocessing unit 910 interfaces with various other components, includingrandom access memory 934 and read only memory 932, which can storenavigational information as well as vehicle control algorithms. In orderto control the vehicle, the guidance processing unit 910 can receiveinput from various sources including an inertial measurement system 922as well as a navigation receiver 920 which may be a global positioningreceiver (GPS) or other type of receiver for providing navigationinformation. The guidance processing unit 910 can also be connected toan uplink data receiver 924 for receiving control commands and datacommunications from the ground or other vehicles and may further beconnected to an onboard seeker such as a radar device, forward-lookinginfrared receiver (FLIR) or other type of target-locating sensor.Finally, the guidance processing unit 910 outputs commands to a vehicleattitude control system 928 and/or aerodynamic maneuvering system 930 inorder to control the vehicle based on the current vehicle position andcondition, the presence and location of any obstacles, and thenavigation objectives to be achieved.

[0201] Piloted Vehicle Automatic Guidance Control Processor

[0202]FIG. 23 is an example of a configuration of a vehicle guidancecontrol system implementing the above described spatial avoidance methodwhen the vehicle is piloted but under automatic guidance control. FIG.23 illustrates the method of the invention used in systems which areknown as fly-by-wire type systems. In a fly-by-wire type system, theavoidance commands are automatically directed to the autopilot function916 without required intervention by the pilot/user 917. The pilot/user917 acts as an input to the guidance system and does not directly flythe aircraft. All instructions from the pilot are fed through anautopilot function to produce the correct steering commands to thevehicle. In some instances, the pilot/user may be the primary guidanceprocessor with the spatial avoidance method intervening only whennecessary. Most modern aircraft operate with a fly-by-wire controlsystem. The method may also be implemented such that it includes afeedback mechanism 919, which will show the pilot/user the change beingmade by the automatic system relative to the pilot/user input.

[0203] Pilot Controlled Guidance Control Processor

[0204]FIG. 24 is an example of a configuration of a vehicle guidancecontrol system implementing the above described spatial avoidance methodwhen the vehicle is piloted and the pilot 917 is responsible forinputting the guidance control commands calculated by the method such asin a flight director system. In the example of FIG. 24, the pilot/useris in primary control of the system and the guidance processorimplementing the method of the invention only provides feedback to thepilot/user as to how to avoid the obstacles. The feedback mechanism 919may be through an analog or digital indicator which also may be on aninstrument panel or implemented in a heads-up display. The pilot/userultimately makes the determination whether or not to implement theguidance control commands.

[0205] It will be appreciated by those of skill in the art, that thecontrol laws for the above described system in accordance with thepresent invention can be implemented in computer software and stored ina memory device. Thus, a computer program encoding instructions inaccordance with the described method, as well as a computer readablememory device which stores instructions executable by a computer toperform the steps of the described method are also considered to beembodiments of the present invention.

[0206] Although the present invention has been fully described by way ofexamples and with reference to the accompanying drawings, it is to beunderstood that various changes and modifications will be apparent tothose skilled in the art without departing from the spirit and scope ofthe invention. Therefore, unless such changes and modifications departfrom the scope of the present invention, they should be construed asbeing included therein.

What is claimed is:
 1. A method of maneuvering a vehicle towards adestination in the presence of at least one obstacle, comprising thesteps of: establishing a nominal guidance direction for said vehicle;establishing a nominal obstacle avoidance direction based on saidobstacle; determining a vehicle steering direction based on saidguidance direction and said obstacle avoidance direction.
 2. A method inaccordance with claim 1, wherein said nominal obstacle avoidancedirection is a geometric based avoidance vector determined based on adefined geometric boundary surrounding said obstacle.
 3. A method inaccordance with claim 1, further comprising the steps of: defining ageometric boundary surrounding said obstacle; and determining whethersaid vehicle is outside of said geometric boundary or inside of saidgeometric boundary.
 4. A method in accordance with claim 3, wherein, inthe case that said vehicle is outside of said geometric boundary, saidnominal guidance direction for said vehicle is a direction substantiallytowards said destination.
 5. A method in accordance with claim 2,wherein, in the case that said vehicle is inside of said geometricboundary, said nominal guidance direction for said vehicle is ananti-intercept direction between said vehicle and said obstacle.
 6. Amethod in accordance with claim 2, further comprising the step ofdetermining a distance between said vehicle and said obstacle.
 7. Amethod in accordance with claim 6, wherein said step of determining avehicle steering direction is further based on said distance.
 8. Amethod in accordance with claim 7, wherein said vehicle steeringdirection is further based on a distance-based field element.
 9. Amethod in accordance with claim 7, further comprising the step ofdetermining a range of influence of said obstacle and wherein, in thecase where said vehicle is beyond the range of influence of saidobstacle, said vehicle steering direction is determined without regardto the presence of said obstacle.
 10. A method in accordance with claim7, further comprising the step of determining a range of influence ofsaid obstacle and wherein, in the case where said vehicle is within therange of influence of said obstacle, said vehicle steering direction isdetermined based on the presence of said obstacle.
 11. A method inaccordance with claim 1, wherein said steering direction is determinedby blending together said nominal guidance direction and said obstacleavoidance direction based on a blending ratio.
 12. A method inaccordance with claim 11, wherein said blending ratio is a function. 13.A method in accordance with claim 12, wherein said blending ratio is afunction of a distance separating said vehicle and said obstacle.
 14. Amethod in accordance with claim 11, wherein said steering direction isdetermined in accordance with a function of the form: D _(STEERING) =F1*D _(GUIDANCE) +F 2*D _(AVOIDANCE); where: D_(GUIDANCE) is a vectorwith a direction in said nominal guidance direction; D_(AVOIDANCE) is avector with a direction in said nominal obstacle avoidance direction;D_(STEERING) is said vehicle steering direction; F1 is a firstcontinuous function; and F2 is a second continuous function.
 15. Amethod in accordance with claim 14, wherein F2≈1−F1.
 16. A method inaccordance with claim 14, further comprising the step of steering saidvehicle towards said steering direction; and wherein said nominalobstacle avoidance direction is based on a relative position of saidvehicle to said obstacle, and wherein said steps of establishing anominal guidance direction towards said destination, establishing anominal obstacle avoidance direction based on said obstacle, determininga vehicle steering direction based on said guidance direction and saidobstacle avoidance direction, and steering said vehicle towards saidsteering direction are repeated a plurality of times as said vehicletravels.
 17. A method in accordance with claim 1, wherein: said vehicletravels towards said destination in the presence of a plurality ofobstacles; said step of establishing a nominal obstacle avoidancedirection includes establishing a nominal obstacle avoidance directionfor each of said obstacles; and said step of determining a vehiclesteering direction includes determining a vehicle steering directionbased on said guidance direction and each of said obstacle avoidancedirections.
 18. A method in accordance with claim 2, further comprisingthe step of determining whether said vehicle is approaching saidobstacle or whether said vehicle is receding from said obstacle.
 19. Amethod in accordance with claim 18, wherein said step of determining avehicle steering direction includes determining a vehicle steeringdirection based on a result of said step of determining whether saidvehicle is approaching said obstacle or whether said vehicle is recedingfrom said obstacle.
 20. A method in accordance with claim 1, furthercomprising the step outputting a steering control signal effective tosteer said vehicle in accordance with said steering direction.
 21. Amethod in accordance with claim 1, further comprising the step ofdisplaying a steering command in accordance with said steeringdirection.
 22. A method of maneuvering a vehicle towards a destinationin the presence of at least one obstacle, comprising the steps of:establishing a nominal guidance direction for said vehicle; establishinga geometric boundary surrounding said obstacle; determining a distancebetween said vehicle and said obstacle; determining whether said vehicleis inside of said geometric boundary or outside of said geometricboundary; based on said step of determining whether said vehicle isinside of said geometric boundary or outside of said geometric boundary,establishing: in the case where said vehicle is inside of said geometricboundary, said nominal guidance direction for said vehicle is adirection substantially towards the destination, and in the case wheresaid vehicle is inside of said geometric boundary, said nominal guidancedirection for said vehicle is an anti intercept direction between saidvehicle and said obstacle; and determining a vehicle steering directionbased on said guidance direction, an applicable one of said nominalguidance direction based on a direction substantially towards saiddestination and said nominal guidance direction based on ananti-intercept direction, said distance, and any distance-based fieldelement.
 23. An apparatus for maneuvering a vehicle towards adestination in the presence of at least one obstacle, comprising: acontroller having a memory; a means for inputting a nominal guidancedirection for said vehicle into said memory; a computer for computing anominal obstacle avoidance direction based on said obstacle, and forcomputing a vehicle steering direction based on said guidance directionand said obstacle avoidance direction.
 24. An apparatus of claim 23,further comprising: an autopilot for receiving said vehicle steeringdirection and steering said vehicle toward said vehicle steeringdirection.
 25. A method for maneuvering a first vehicle in the presenceof a second vehicle, comprising the steps of: establishing a nominalguidance direction for each of said first and said second vehicles;establishing a nominal obstacle avoidance direction for said firstvehicle with respect to said second vehicle; determining a steeringdirection for said first vehicle based on said guidance direction ofsaid first vehicle and said obstacle avoidance direction.
 26. A methodfor maneuvering a first vehicle in relation to a second vehicle,comprising the steps of: establishing a first repulsive boundary aroundsaid second vehicle; establishing a first attractive boundary aroundsaid first repulsive boundary, said first repulsive boundary and saidfirst attractive boundary defining an interactive region therebetween;computing a steering direction for said first vehicle such that whensaid first vehicle is outside said first attractive boundary, said firstvehicle is directed toward said second vehicle, when said first vehicleis within said first repulsive boundary, said first vehicle is directedaway from said second vehicle, and when said first vehicle is withinsaid interactive region, said first vehicle is directed either toward oraway from said second vehicle based on a weighted function.
 27. Aguidance processor for maneuvering a vehicle towards a destination inthe presence of at least one obstacle, comprising: a device fordetermining a nominal guidance direction for said vehicle; a device fordetermining a nominal obstacle avoidance direction based on saidobstacle; a device for determining a vehicle steering direction based onsaid guidance direction and said obstacle avoidance direction.
 28. Aguidance processor in accordance with claim 27, wherein said nominalobstacle avoidance direction is a geometric based avoidance vectordetermined based on a defined geometric boundary surrounding saidobstacle.
 29. A guidance processor in accordance with claim 27, furthercomprising: a device for defining a geometric boundary surrounding saidobstacle; and a device for determining whether said vehicle is outsideof said geometric boundary or inside of said geometric boundary.
 30. Aguidance processor in accordance with claim 29, wherein, in the casethat said vehicle is outside of said geometric boundary, said nominalguidance direction for said vehicle is a direction substantially towardssaid destination.
 31. A guidance processor in accordance with claim 29,wherein, in the case that said vehicle is inside of said geometricboundary, said nominal guidance direction for said vehicle is ananti-intercept direction between said vehicle and said obstacle.
 32. Aguidance processor in accordance with claim 28, further comprising: adevice for determining a distance between said vehicle and saidobstacle.
 33. A guidance processor in accordance with claim 32, whereinsaid device for determining a vehicle steering direction furtherdetermines said vehicle steering direction based on said distance.
 34. Aguidance processor in accordance with claim 33, wherein said device fordetermining a vehicle steering direction further determines said vehiclesteering direction based on a distance-based field element.
 35. Aguidance processor in accordance with claim 33, further comprising: adevice for determining a range of influence of said obstacle andwherein, in the case where said vehicle is beyond the range of influenceof said obstacle, said device for determining a vehicle steeringdirection further determines said vehicle steering direction withoutregard to the presence of said obstacle.
 36. A guidance processor inaccordance with claim 33, further comprising: a device for determining arange of influence of said obstacle and wherein, in the case where saidvehicle is within the range of influence of said obstacle, said devicefor determining a vehicle steering direction further determines saidvehicle steering direction based on the presence of said obstacle.
 37. Aguidance processor in accordance with claim 27, wherein said device fordetermining a vehicle steering direction determines said steeringdirection by blending together said nominal guidance direction and saidobstacle avoidance direction based on a blending ratio.
 38. A guidanceprocessor in accordance with claim 37, wherein said blending ratio is afunction.
 39. A guidance processor in accordance with claim 38, whereinsaid blending ratio is a function of a distance separating said vehicleand said obstacle.
 40. A guidance processor in accordance with claim 37,wherein said device for determining a vehicle steering directiondetermines said steering direction in accordance with a function of theform: D _(STEERING) =F 1*D _(GUIDANCE) +F 2*D _(AVOIDANCE); where:D_(GUIDANCE) is a vector with a direction in said nominal guidancedirection; D_(AVOIDANCE) is a vector with a direction in said nominalobstacle avoidance direction; D_(STEERING) is said vehicle steeringdirection; F1 is a first continuous function; and F2 is a secondcontinuous function.
 41. A guidance processor in accordance with claim40, wherein F2≈1−F1.
 42. A guidance processor in accordance with claim40, further comprising: a device for steering said vehicle towards saidsteering direction; and wherein said device for determining a nominalguidance direction for said vehicle determines said nominal obstacleavoidance direction based on a relative position of said vehicle to saidobstacle, and wherein device for determining a nominal guidancedirection for said vehicle, said device for determining a nominalobstacle avoidance direction based on said obstacle, and said device fordetermining a vehicle steering direction each continually update itscorresponding direction as said vehicle travels.
 43. A guidanceprocessor in accordance with claim 27, wherein: said vehicle travelstowards said destination in the presence of a plurality of obstacles;said device for determining a nominal obstacle avoidance directionincludes establishing a nominal obstacle avoidance direction determinesa nominal obstacle avoidance direction for each of said obstacles; andsaid device for determining a vehicle steering direction determines avehicle steering direction based on said guidance direction and each ofsaid obstacle avoidance directions.
 44. A guidance processor inaccordance with claim 28, further comprising: a device for determiningwhether said vehicle is approaching said obstacle or whether saidvehicle is receding from said obstacle.
 45. A guidance processor inaccordance with claim 44, wherein said device for determining a vehiclesteering direction determines a vehicle steering direction based on anoutput from said device for determining whether said vehicle isapproaching said obstacle or whether said vehicle is receding from saidobstacle.
 46. A guidance processor in accordance with claim 27, furthercomprising: a device for outputting a steering control signal to steersaid vehicle in accordance with said steering direction.
 47. A guidanceprocessor in accordance with claim 27, further comprising: a device fordisplaying a steering command in accordance with said steeringdirection.
 48. A memory device for use with a guidance processor formaneuvering a vehicle towards a destination in the presence of at leastone obstacle, said memory device comprising encoded instructionsexecutable by the guidance processor for: establishing a nominalguidance direction for said vehicle; establishing a nominal obstacleavoidance direction based on said obstacle; determining a vehiclesteering direction based on said guidance direction and said obstacleavoidance direction.
 49. A memory device in accordance with claim 48,wherein said nominal obstacle avoidance direction is a geometric basedavoidance vector determined based on a defined geometric boundarysurrounding said obstacle.
 50. A memory device in accordance with claim48, further comprising encoded instructions executable by the autopilotcontroller for: defining a geometric boundary surrounding said obstacle;and determining whether said vehicle is outside of said geometricboundary or inside of said geometric boundary.
 51. A memory device inaccordance with claim 50, wherein, in the case that said vehicle isoutside of said geometric boundary, said nominal guidance direction forsaid vehicle is a direction substantially towards said destination. 52.A memory device in accordance with claim 50, wherein, in the case thatsaid vehicle is inside of said geometric boundary, said nominal guidancedirection for said vehicle is an anti-intercept direction between saidvehicle and said obstacle.
 53. A memory device in accordance with claim49, further comprising encoded instructions executable by the guidanceprocessor for: receiving information indicative of a distance betweensaid vehicle and said obstacle.
 54. A memory device in accordance withclaim 53, wherein said encoded instructions for determining a vehiclesteering direction further determine said steering direction based onsaid distance.
 55. A memory device in accordance with claim 54, whereinsaid encoded instructions for determining a vehicle steering directionfurther determine said vehicle steering direction based on adistance-based field element.
 56. A memory device in accordance withclaim 54, further comprising encoded instructions executable by theguidance processor for: determining a range of influence of saidobstacle; and wherein, in the case where said vehicle is beyond therange of influence of said obstacle, said encoded instructions fordetermining a vehicle steering direction further determine said vehiclesteering direction without regard to the presence of said obstacle. 57.A memory device in accordance with claim 54, further comprising encodedinstructions executable by the guidance processor for: determining arange of influence of said obstacle and wherein, in the case where saidvehicle is within the range of influence of said obstacle, said encodedinstructions for determining a vehicle steering direction furtherdetermine said vehicle steering direction based on the presence of saidobstacle.
 58. A memory device in accordance with claim 48, wherein saidencoded instructions for determining a vehicle steering directiondetermine said steering direction by blending together said nominalguidance direction and said obstacle avoidance direction based on ablending ratio.
 59. A memory device in accordance with claim 58, whereinsaid blending ratio is a function.
 60. A memory device in accordancewith claim 59, wherein said blending ratio is a function of a distanceseparating said vehicle and said obstacle.
 61. A memory device inaccordance with claim 58, wherein said steering direction is determinedin accordance with a function of the form: D _(STEERING) =F 1*D_(GUIDANCE) +F 2*D _(AVOIDANCE); where: D_(GUIDANCE) is a vector with adirection in said nominal guidance direction; D_(AVOIDANCE) is a vectorwith a direction in said nominal obstacle avoidance direction;D_(STEERING) is said vehicle steering direction; F1 is a firstcontinuous function; and F2 is a second continuous function.
 62. Amemory device in accordance with claim 61, wherein F2≈1−F1.
 63. A memorydevice in accordance with claim 61, further comprising encodedinstructions executable by the guidance processor for: outputting asteering signal for steering said vehicle towards said steeringdirection; and wherein said encoded instructions for determining anominal obstacle avoidance direction determine said nominal obstacleavoidance direction based on a relative position of said vehicle to saidobstacle, and wherein said memory further comprises encoded instructionsto execute said instructions for establishing a nominal guidancedirection towards said destination, establishing a nominal obstacleavoidance direction based on said obstacle, determining a vehiclesteering direction based on said guidance direction and said obstacleavoidance direction, and outputting a steering signal for steering saidvehicle towards said steering direction a plurality of times as saidvehicle travels.
 64. A memory device in accordance with claim 48,wherein: said vehicle travels towards said destination in the presenceof a plurality of obstacles; said encoded instructions for establishinga nominal obstacle avoidance direction include encoded instructions forestablishing a nominal obstacle avoidance direction for each of saidobstacles; and said encoded instructions for determining a vehiclesteering direction includes encoded instructions for determining avehicle steering direction based on said guidance direction and each ofsaid obstacle avoidance directions.
 65. A memory device in accordancewith claim 49, further comprising encoded instructions executable by theguidance processor for: determining whether said vehicle is approachingsaid obstacle or whether said vehicle is receding from said obstacle.66. A memory device in accordance with claim 65, wherein said encodedinstructions for determining a vehicle steering direction includesencoded instructions for determining a vehicle steering direction basedon an output of said encoded instructions for determining whether saidvehicle is approaching said obstacle or whether said vehicle is recedingfrom said obstacle.
 67. A memory device in accordance with claim 48,further comprising encoded instructions executable by the guidanceprocessor for: outputting a steering control signal effective to steersaid vehicle in accordance with said steering direction.
 68. A memorydevice in accordance with claim 48, further comprising encodedinstructions executable by the guidance processor for: outputting asteering display command in accordance with said steering direction forindicating a direction for steering said vehicle.
 69. Computerexecutable software for use with a guidance processor for maneuvering avehicle towards a destination in the presence of at least one obstacle,comprising: means for determining a nominal guidance direction for saidvehicle; means for determining a nominal obstacle avoidance directionbased on said obstacle; means for determining a vehicle steeringdirection based on said guidance direction and said obstacle avoidancedirection.
 70. An autopilot controller for maneuvering a vehicle towardsa destination in the presence of at least one obstacle, comprising: adevice for determining a nominal guidance direction for said vehicle; adevice for determining a nominal obstacle avoidance direction based onsaid obstacle; a device for determining a vehicle steering directionbased on said guidance direction and said obstacle avoidance direction.71. An autopilot controller in accordance with claim 70, wherein saidnominal obstacle avoidance direction is a geometric based avoidancevector determined based on a defined geometric boundary surrounding saidobstacle.
 72. An autopilot controller in accordance with claim 70,wherein said device for determining a vehicle steering directiondetermines said steering direction by blending together said nominalguidance direction and said obstacle avoidance direction based on ablending ratio.
 73. An autopilot controller in accordance with claim 72,wherein said blending ratio is a function.
 74. An autopilot controllerin accordance with claim 73, wherein said blending ratio is a functionof a distance separating said vehicle and said obstacle.
 75. Anautopilot controller in accordance with claim 72, wherein said devicefor determining a vehicle steering direction determines said steeringdirection in accordance with a function of the form: D _(STEERING) =F1*D _(GUIDANCE) +F 2*D _(AVOIDANCE); where: D_(GUIDANCE) is a vectorwith a direction in said nominal guidance direction; D_(AVOIDANCE) is avector with a direction in said nominal obstacle avoidance direction;D_(STEERING) is said vehicle steering direction; F1 is a firstcontinuous function; and F2 is a second continuous function.
 76. Anautopilot controller in accordance with claim 75, wherein F2≈1−F1. 77.An autopilot controller in accordance with claim 75, further comprising:a device for steering said vehicle towards said steering direction; andwherein said device for determining a nominal guidance direction forsaid vehicle determines said nominal obstacle avoidance direction basedon a relative position of said vehicle to said obstacle, and whereindevice for determining a nominal guidance direction for said vehicle,said device for determining a nominal obstacle avoidance direction basedon said obstacle, and said device for determining a vehicle steeringdirection each continually update its corresponding direction as saidvehicle travels.